package nape.constraint;
import nape.phys.PhysObj;
import nape.space.Space;
import nape.geom.VecMath;
import nape.geom.Vec2;
import nape.constraint.ClassicCons;
import nape.constraint.Constraint;
import nape.Const;
import nape.util.FastMath;
















class PivotJoint extends ClassicCons {
	
	mat_new(public,kt);
	
	vec_new(public,bias);
	vec_new(public,jAcc);
	
	public var jMax:Float;
	
	
	
	public function new(obj1:PhysObj,obj2:PhysObj,pivot:Vec2) {
		super(obj1,obj2,pivot,pivot);
		vec_set(jAcc,0,0);
	}
	
	
	
	public override function preStep(dt:Float) {
		ranchor(a1,b1,r1);
		ranchor(a2,b2,r2);
		rdelta(b1,b2,r1,r2,bias);
		KTensor(b1,b2,kt);
		
		vec_muleq(bias,-biasCoef/dt);
		vec_clamp(bias,maxBias);
		
		jMax = maxForce*dt;
		AppImp(b1,b2,r1,r2,jAcc,);
		
		return false;
	}
	
	
	public override function applyImpulse():Bool {
		vec_new(vr); calc_relvel(b1,b2,r1,r2,vr,);
		
		vec_new(j); vec_sub(bias,vr,j);
		mat_transform(kt,j);
		vec_new(jOld); vec_set(jOld,jAcc); vec_addeq(jAcc,j);
		
		if(breakable) { if(vec_lsq(jAcc)>jMax*jMax) return true; }
		vec_clamp(jAcc,jMax);

		vec_sub(jAcc,jOld,j);
		AppImp(b1,b2,r1,r2,j,);
		return false;
	}
	
	
	
	public override function impulse(obj:PhysObj,dst:Vec2) {
		if(obj==b1) vec_set(dst.p,-jAccx,-jAccy);
		else      vec_set(dst.p, jAcc);
	}
}